#ifndef REMOTE_CONTROL_CORE_H
#define REMOTE_CONTROL_CORE_H

#include "ros/ros.h"
#include "remote_control/vehicleToRemote.h"
#include "remote_control/Point2D.h"
#include "costmap_compress/CompressedOccupancyGrid.h"
#include "robocoachMsg/GnssStatus.h"
#include "robocoachMsg/CarInfo.h"
#include "openssl/md5.h"
#include <iomanip>
#include <sstream>
#include <cmath> 
#include <vector>
#include <utility> // 用于 std::pair
#include <visualization_msgs/Marker.h>
#include <geometry_msgs/Point.h>
#include <std_msgs/ColorRGBA.h>
#include <iterator> // 用于 std::distance
#include <fstream>
#include <jsoncpp/json/json.h>
#include <nav_msgs/OccupancyGrid.h>


#define __APP_NAME__ "remote_control"

class RemoteControl
{
    public:
        RemoteControl();
        ~RemoteControl();
        void run();
    
    private:
        ros::NodeHandle nh_;
        ros::NodeHandle private_nh_;

    private:
        std::string input_costmapCompress_topic_;
        std::string input_gnssStatus_topic;
        std::string input_carInfo_topic_;

        std::string output_vehicleToRemote_topic_;

    private:
        ros::Subscriber costmapCompress_sub_;
        ros::Subscriber gnssStatus_sub_;
        ros::Subscriber carInfo_sub_;
        costmap_compress::CompressedOccupancyGrid costmap_compress_msg_;
        robocoachMsg::GnssStatus gnss_status_msg_;
        robocoachMsg::CarInfo car_info_msg_;

    private:
        ros::Publisher remoteControl_pub_;
        ros::Publisher marker_pub_;

    private:
        int outfile_start_count = 0;
        int obstacle_maxnum = 0;

    void costmapCompress_callback(const costmap_compress::CompressedOccupancyGrid::ConstPtr& msg);
    void gnssStatus_callback(const robocoachMsg::GnssStatus::ConstPtr& msg);
    void carInfo_callback(const robocoachMsg::CarInfo::ConstPtr& msg);
    //void obstacleAnalysis(const remote_control::vehicleToRemote& msg);
    std::vector<std::pair<double, double>> obstacleAnalysis(const remote_control::vehicleToRemote& msg, const std::vector<uint8_t>& compressed_data);
    std::vector<std::pair<double, double>> fourCornersAnalysis(const remote_control::vehicleToRemote& msg);
    void process(const ros::TimerEvent& e);

};



#endif
